/*******************************************************************************
* FILE NAME: user_routines_fast.c <VEX VERSION>
*
* DESCRIPTION:
*  This file is where the user can add their custom code within the framework
*  of the routines below.
*
* USAGE:
*  You can either modify this file to fit your needs, or remove it from your
*  project and replace it with a modified copy.
*
* OPTIONS:  Interrupts are disabled and not used by default.
*
*******************************************************************************/
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "printf_lib.h"
#include "delays.h"
#include "lovitt_routines.h"
#include "user_utilities.h"
#include "patrick_routines.h"
#include "mark_routines.h"
#include "user_utilities.h"

/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/
int currentPosition[3]; //holds x and y position, also turn position in radians
signed int flEncoder, frEncoder, blEncoder, brEncoder; //fl, fr, bl, br wheel counters
signed long tmr0_Count; //timer counter for finding velocity
int autonomous_Stage; //current autonomous stage
int portPast = 0;
extern char virtControlLeftX, virtControlLeftY, virtControlRightX;
int counter;
int counter2;
int autoSlide;
int autoDrive;
int autoClawLift;
int autoClawSweep;
int autoHoard;
int autoPush;
int liftPosition;

/*
 * -------  Function prototypes ---------------
 */
void drive(char forwardOrBackward, int howFar);
void slide(char leftOrRight, int howFar);
void turn(char leftOrRight, float degrees);
void lift(char upOrDown, int destination);
void sweep(char leftOrRight, int time);
void hoard(char leftOrRight, int howFar);
void push(char forwardOrBackward, int howFar);
void endStage();

/*******************************************************************************
* FUNCTION NAME: InterruptVectorLow
* PURPOSE:       Low priority interrupt vector
* CALLED FROM:   nowhere by default
* ARGUMENTS:     none
* RETURNS:       void
* DO NOT MODIFY OR DELETE THIS FUNCTION
*******************************************************************************/
#pragma code InterruptVectorLow = LOW_INT_VECTOR
void InterruptVectorLow (void)
{
  _asm
    goto InterruptHandlerLow  /*jump to interrupt routine*/
  _endasm
}
#pragma code
#pragma interruptlow InterruptHandlerLow save=PROD /* You may want to save additional symbols. */

/*******************************************************************************
* FUNCTION NAME: InterruptHandlerLow
* PURPOSE:       Low priority interrupt handler
* If you want to use these external low priority interrupts or any of the
* peripheral interrupts then you must enable them in your initialization
* routine.  Innovation First, Inc. will not provide support for using these
* interrupts, so be careful.  There is great potential for glitchy code if good
* interrupt programming practices are not followed.  Especially read p. 28 of
* the "MPLAB(R) C18 C Compiler User's Guide" for information on context saving.
* CALLED FROM:   this file, InterruptVectorLow routine
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void InterruptHandlerLow ()
{
  unsigned char int_byte;

  //================ON CHIP TIMER 0 =================
  if ( (INTCONbits.TMR0IE) && (INTCONbits.TMR0IF) ) /*Timer flag enabled & flag is set*/
  {
	 /* tmr0_Count += 1;
	  TMR0L = 0x05;*/
	  INTCONbits.TMR0IF = 0;
  }
  //===============FRONT LEFT WHEEL COUNTER===================
  else if (INTCON3bits.INT2IF && INTCON3bits.INT2IE)       /* The INT2 pin is RB2/DIG I/O 1. */
  {
	/*  if (!FL_BACKWARDS_CHECK)
	  {
		  flEncoder --;
	  }

	  else
	  {
		  flEncoder ++;
	  } */

	  INTCON3bits.INT2IF = 0;   /*Reset the flag (we got it)*/
  }

  // ===============FRONT RIGHT WHEEL COUNTER================
  else if (INTCON3bits.INT3IF && INTCON3bits.INT3IE)  /* The INT3 pin is RB3/DIG I/O 2. */
  {
	/*if(!FR_BACKWARDS_CHECK)
	{
		frEncoder --;
	}

	else
	{
		frEncoder ++;
	}*/

	INTCON3bits.INT3IF = 0;
  }

  //================ON CHIP TIMER 1==============

  else if ( (PIE1bits.TMR1IE) && (PIR1bits.TMR1IF) ) //Timer flag enabled & flag is set
  {

  PIR1bits.TMR1IF = 0;
  }
  //=============================INTERRUPTS 4-6=======================
  else if (INTCONbits.RBIF && INTCONbits.RBIE)  /* DIG I/O 3-6 (RB4, RB5, RB6, or RB7) changed. */
  {
	/*  char portCurrent = PORTB;
	  char combinedLatch = portPast^portCurrent;

	  //printf("bl int: %16b bl lat: %16b bl end: %16b \n", (int)portPast, (int)portCurrent, (int)combinedLatch);

	  if (combinedLatch & 16 && portCurrent & 16)
	  {
		  if(!BL_BACKWARDS_CHECK)
		  {
			  blEncoder --;
		  }

		  else
		  {
			  blEncoder ++;
		  }
	  }

	  if (combinedLatch & 32 && portCurrent & 32)
	  {
		  if(!BR_BACKWARDS_CHECK)
		  {
			  brEncoder --;
		  }

		  else
		  {
			  brEncoder ++;
		  }
	  }

	  portPast = portCurrent; */

	  int_byte = PORTB;          /* You must read or write to PORTB */
	  INTCONbits.RBIF = 0;       /* and clear the interrupt flag*/
  }                           /*  to clear the interrupt condition.*/
}



/*******************************************************************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE:       Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode.  It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM:   main.c file, main() routine when in Autonomous mode
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void User_Autonomous_Code(void)
{
  /* Initialize all PWMs and Relays when entering Autonomous mode, or else it
     will be stuck with the last values mapped from the joysticks.  Remember,
     even when Disabled it is reading inputs from the Operator Interface.
  */
  pwm01 = pwm02 = pwm03 = pwm04 = 127;
  pwm05 = pwm06 = pwm07 = pwm08 = 127;
  pwm09 = pwm10 = pwm11 = pwm12 = 127;


  counter2 = 0;

  while (autonomous_mode)   /* DO NOT CHANGE! */
  {
	if (statusflag.NEW_SPI_DATA)      //* 18.5ms loop area 0
    {
      Getdata(&rxdata);   //* DO NOT DELETE, or you will be stuck here forever!
 /*
  * 1. drive it to the other side  0 then  OR drive it to the other side of your field
  * 1.b.Drive the robot forward to the wall
  * 2. Lift claw and turn it for a given time to knock down the three orange balls in front of it
  *    onto the opponent's side.
  * 3. Drive right (or left) and knock the three orange balls in the middle to the opponent's side,
  *    while herding the  five orange balls on your side  and the five green balls at the middle of
  *    the wall to a place where the can be easy to work with after autonomous.
  *	4. When at the other side of the wall, do the same thing as you did with the first side with
  *	   the orange balls, roll the green balls through.
  *
  *	 */
  	   EXAMPLE:
  	   This will get the potentiometer value from digital/analog port 1
  	   x = get_Analog_Value(rc_ana_in01);

//        autoSlide = 980;
        autoDrive = 490;
/*        autoClawLift = 42;
        autoClawSweep = 56;
        autoHoard = 900;
        autoPush = 15;

		if (rc_dig_in09 == 0 ||rc_dig_in10 == 0 || rc_dig_in11 == 0 || rc_dig_in12 == 0 || rc_dig_in13 == 0){
			if (rc_dig_in04 == 0 ||rc_dig_in05 == 0 || rc_dig_in06 == 0 || rc_dig_in07 == 0 || rc_dig_in08 == 0){
				autoSlide = 0;
				autoHoard = autoHoard * -1;
			}

			else{
				autoSlide = autoSlide * -1;
			}
		}

		else{
			if (rc_dig_in04 == 0 ||rc_dig_in05 == 0 || rc_dig_in06 == 0 || rc_dig_in07 == 0 || rc_dig_in08 == 0){
				autoSlide = 0;
			}
			else{
				autoHoard = autoHoard * -1;
			}
		}

		if(autonomous_Stage == 1){
			slide(RIGHT, autoSlide);
		}  */

//		if(autonomous_Stage == 2){
        if (counter2 < 400){
        	// drive backwards (400 count)
        	FRONT_LEFT_WHEEL_MOTOR = 0;
        	FRONT_RIGHT_WHEEL_MOTOR = 255;
        	BACK_LEFT_WHEEL_MOTOR = 0;
        	BACK_RIGHT_WHEEL_MOTOR = 255;
		}

        else{
        	FRONT_LEFT_WHEEL_MOTOR = 127;
        	FRONT_RIGHT_WHEEL_MOTOR = 127;
        	BACK_LEFT_WHEEL_MOTOR = 127;
        	BACK_RIGHT_WHEEL_MOTOR = 127;
        }
		//puts shovel down (130 count)
        if(counter2 > 25 && counter2 < 155){
        	LEFT_LIFT_FORK_MOTOR = 255;
        	RIGHT_LIFT_FORK_MOTOR = 0;
        }
        else{
        	LEFT_LIFT_FORK_MOTOR = 127;
        	RIGHT_LIFT_FORK_MOTOR = 127;
        }
        //dumps the hopper (120 count)
        if(counter2 > 400 && counter2 < 520){
        	LEFT_LIFT_HOPPER_MOTOR = 255;
        	RIGHT_LIFT_HOPPER_MOTOR = 255;
        }
        else{
        	LEFT_LIFT_HOPPER_MOTOR = 127;
        	RIGHT_LIFT_HOPPER_MOTOR = 127;
        }
        //drives torward the middle (40 count)
        if (counter2 > 520 && counter2 < 760){
             FRONT_LEFT_WHEEL_MOTOR = 255;
             FRONT_RIGHT_WHEEL_MOTOR = 0;
             BACK_LEFT_WHEEL_MOTOR = 255;
             BACK_RIGHT_WHEEL_MOTOR = 0;
        }
        else{
             FRONT_LEFT_WHEEL_MOTOR = 127;
             FRONT_RIGHT_WHEEL_MOTOR = 127;
             BACK_LEFT_WHEEL_MOTOR = 127;
             BACK_RIGHT_WHEEL_MOTOR = 127;
        }

        //turns 90 degrees (50 count)
        if(counter2 > 760 && counter2 < 810){
        	FRONT_LEFT_WHEEL_MOTOR = 255;
        	FRONT_RIGHT_WHEEL_MOTOR = 255;
        	BACK_LEFT_WHEEL_MOTOR = 0;
        	BACK_RIGHT_WHEEL_MOTOR = 0;
        }
        else{
             	FRONT_LEFT_WHEEL_MOTOR = 127;
             	FRONT_RIGHT_WHEEL_MOTOR = 127;
             	BACK_LEFT_WHEEL_MOTOR = 127;
             	BACK_RIGHT_WHEEL_MOTOR = 127;
        }
		//also puts hopper back down while turning (50 count)
        if(counter2 > 810 && counter2 < 860){
        	LEFT_LIFT_HOPPER_MOTOR = 0;
        	RIGHT_LIFT_HOPPER_MOTOR = 0;

        }
        else{
        	LEFT_LIFT_HOPPER_MOTOR = 127;
        	RIGHT_LIFT_HOPPER_MOTOR = 127;
        }
        //drives torward footballs (40 count)
        if (counter2 > 860 && counter2 < 900){
             FRONT_LEFT_WHEEL_MOTOR = 255;
             FRONT_RIGHT_WHEEL_MOTOR = 0;
             BACK_LEFT_WHEEL_MOTOR = 255;
             BACK_RIGHT_WHEEL_MOTOR = 0;
        }
        else{
             FRONT_LEFT_WHEEL_MOTOR = 127;
             FRONT_RIGHT_WHEEL_MOTOR = 127;
             BACK_LEFT_WHEEL_MOTOR = 127;
             BACK_RIGHT_WHEEL_MOTOR = 127;
        }

        //shovel up (130 count)
        if(counter2 > 900 && counter2 < 1030){
              LEFT_LIFT_FORK_MOTOR = 255;
              RIGHT_LIFT_FORK_MOTOR = 0;
        }
        else{
		      LEFT_LIFT_FORK_MOTOR = 127;
              RIGHT_LIFT_FORK_MOTOR = 127;
		}

        //shovel down (30 count)
        if(counter2 > 1030 && counter2 < 1060){
              LEFT_LIFT_FORK_MOTOR = 255;
              RIGHT_LIFT_FORK_MOTOR = 0;
        }
        else{
		      LEFT_LIFT_FORK_MOTOR = 127;
              RIGHT_LIFT_FORK_MOTOR = 127;
		}

        //drives toward footballs again (40 count)
         if (counter2 > 1060 && counter2 < 1100){
              FRONT_LEFT_WHEEL_MOTOR = 255;
              FRONT_RIGHT_WHEEL_MOTOR = 0;
              BACK_LEFT_WHEEL_MOTOR = 255;
              BACK_RIGHT_WHEEL_MOTOR = 0;
         }
         else{
              FRONT_LEFT_WHEEL_MOTOR = 127;
              FRONT_RIGHT_WHEEL_MOTOR = 127;
              BACK_LEFT_WHEEL_MOTOR = 127;
              BACK_RIGHT_WHEEL_MOTOR = 127;
         }

         //shovel up again (130 count)
         if(counter2 > 1100 && counter2 < 1230){
               LEFT_LIFT_FORK_MOTOR = 255;
               RIGHT_LIFT_FORK_MOTOR = 0;
         }
         else{
 		      LEFT_LIFT_FORK_MOTOR = 127;
               RIGHT_LIFT_FORK_MOTOR = 127;
 		}

         //shovel down again (130 count)
         if(counter2 > 1230 && counter2 < 1360){
               LEFT_LIFT_FORK_MOTOR = 255;
               RIGHT_LIFT_FORK_MOTOR = 0;
         }
         else{
 		      LEFT_LIFT_FORK_MOTOR = 127;
               RIGHT_LIFT_FORK_MOTOR = 127;
 		}

         //turns 90 degrees towards wall (50 count)
         if(counter2 > 1360 && counter2 < 1410){
         	FRONT_LEFT_WHEEL_MOTOR = 0;
         	FRONT_RIGHT_WHEEL_MOTOR = 0;
         	BACK_LEFT_WHEEL_MOTOR = 255;
         	BACK_RIGHT_WHEEL_MOTOR = 255;
         }
         else{
              	FRONT_LEFT_WHEEL_MOTOR = 127;
              	FRONT_RIGHT_WHEEL_MOTOR = 127;
              	BACK_LEFT_WHEEL_MOTOR = 127;
              	BACK_RIGHT_WHEEL_MOTOR = 127;
         }

         //drives toward wall (40 count)
          if (counter2 > 1410 && counter2 < 1450){
               FRONT_LEFT_WHEEL_MOTOR = 255;
               FRONT_RIGHT_WHEEL_MOTOR = 0;
               BACK_LEFT_WHEEL_MOTOR = 255;
               BACK_RIGHT_WHEEL_MOTOR = 0;
          }
          else{
               FRONT_LEFT_WHEEL_MOTOR = 127;
               FRONT_RIGHT_WHEEL_MOTOR = 127;
               BACK_LEFT_WHEEL_MOTOR = 127;
               BACK_RIGHT_WHEEL_MOTOR = 127;
          }

          //shovel up to hit footballs over (50 count)
          if(counter2 > 1450 && counter2 < 1500){
                LEFT_LIFT_FORK_MOTOR = 255;
                RIGHT_LIFT_FORK_MOTOR = 0;
          }
          else{
  		      LEFT_LIFT_FORK_MOTOR = 127;
                RIGHT_LIFT_FORK_MOTOR = 127;
  		  }

          //drives away wall (50 count)
            if (counter2 > 1500 && counter2 < 1550){
                 FRONT_LEFT_WHEEL_MOTOR = 0;
                 FRONT_RIGHT_WHEEL_MOTOR = 255;
                 BACK_LEFT_WHEEL_MOTOR = 0;
                 BACK_RIGHT_WHEEL_MOTOR = 255;
            }
            else{
                 FRONT_LEFT_WHEEL_MOTOR = 127;
                 FRONT_RIGHT_WHEEL_MOTOR = 127;
                 BACK_LEFT_WHEEL_MOTOR = 127;
                 BACK_RIGHT_WHEEL_MOTOR = 127;
            }

            //drives sideways-left to hit other footballs (50 count)
              if (counter2 > 1550 && counter2 < 1600){
                   FRONT_LEFT_WHEEL_MOTOR = 0;
                   FRONT_RIGHT_WHEEL_MOTOR = 255;
                   BACK_LEFT_WHEEL_MOTOR = 0;
                   BACK_RIGHT_WHEEL_MOTOR = 255;
              }
              else{
                   FRONT_LEFT_WHEEL_MOTOR = 127;
                   FRONT_RIGHT_WHEEL_MOTOR = 127;
                   BACK_LEFT_WHEEL_MOTOR = 127;
                   BACK_RIGHT_WHEEL_MOTOR = 127;
              }


              //drives toward wall (50 count)
               if (counter2 > 1660 && counter2 < 1650){
                    FRONT_LEFT_WHEEL_MOTOR = 255;
                    FRONT_RIGHT_WHEEL_MOTOR = 0;
                    BACK_LEFT_WHEEL_MOTOR = 255;
                    BACK_RIGHT_WHEEL_MOTOR = 0;
               }
               else{
                    FRONT_LEFT_WHEEL_MOTOR = 127;
                    FRONT_RIGHT_WHEEL_MOTOR = 127;
                    BACK_LEFT_WHEEL_MOTOR = 127;
                    BACK_RIGHT_WHEEL_MOTOR = 127;
               }

               //shovel up to hit footballs over again (50 count)
               if(counter2 > 1650 && counter2 < 1700){
                     LEFT_LIFT_FORK_MOTOR = 255;
                     RIGHT_LIFT_FORK_MOTOR = 0;
               }
               else{
       		      LEFT_LIFT_FORK_MOTOR = 127;
                  RIGHT_LIFT_FORK_MOTOR = 127;
       		   }

               //drives sideways-right to middle of 2 pillar things (20 count)
                 if (counter2 > 1700 && counter2 < 1720){
                      FRONT_LEFT_WHEEL_MOTOR = 0;
                      FRONT_RIGHT_WHEEL_MOTOR = 255;
                      BACK_LEFT_WHEEL_MOTOR = 0;
                      BACK_RIGHT_WHEEL_MOTOR = 255;
                 }
                 else{
                      FRONT_LEFT_WHEEL_MOTOR = 127;
                      FRONT_RIGHT_WHEEL_MOTOR = 127;
                      BACK_LEFT_WHEEL_MOTOR = 127;
                      BACK_RIGHT_WHEEL_MOTOR = 127;
                 }


                 //turns 180 degrees to line up to dump (100 count)
                 if(counter2 > 1720 && counter2 < 1820){
                 	FRONT_LEFT_WHEEL_MOTOR = 0;
                 	FRONT_RIGHT_WHEEL_MOTOR = 0;
                 	BACK_LEFT_WHEEL_MOTOR = 255;
                 	BACK_RIGHT_WHEEL_MOTOR = 255;
                 }
                 else{
                    FRONT_LEFT_WHEEL_MOTOR = 127;
                    FRONT_RIGHT_WHEEL_MOTOR = 127;
                    BACK_LEFT_WHEEL_MOTOR = 127;
                    BACK_RIGHT_WHEEL_MOTOR = 127;
                 }

                 //drives backwards toward wall (50 count)
                  if (counter2 > 1820 && counter2 < 1870){
                       FRONT_LEFT_WHEEL_MOTOR = 255;
                       FRONT_RIGHT_WHEEL_MOTOR = 0;
                       BACK_LEFT_WHEEL_MOTOR = 255;
                       BACK_RIGHT_WHEEL_MOTOR = 0;
                  }
                  else{
                       FRONT_LEFT_WHEEL_MOTOR = 127;
                       FRONT_RIGHT_WHEEL_MOTOR = 127;
                       BACK_LEFT_WHEEL_MOTOR = 127;
                       BACK_RIGHT_WHEEL_MOTOR = 127;
                  }

                  //dumps the hopper (120 count)
                  if(counter2 > 400 && counter2 < 520){
                  	LEFT_LIFT_HOPPER_MOTOR = 255;
                  	RIGHT_LIFT_HOPPER_MOTOR = 255;
                  }
                  else{
                  	LEFT_LIFT_HOPPER_MOTOR = 127;
                  	RIGHT_LIFT_HOPPER_MOTOR = 127;
                  }

/*		if(autonomous_Stage == 3){
 * 			lift(UP, autoClawLift);
 *  	sweep(RIGHT, autoClawSweep);
		}

		if(autonomous_Stage == 3){
			hoard(RIGHT, autoHoard);
		}

		if(autonomous_Stage == 4){
			push(FORWARD, autoPush);
		}
*/

	Putdata(&txdata);   /* DO NOT DELETE, or you will get no PWM outputs! */
    }
  }
}

/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Local_IO
* PURPOSE:       Execute user's realtime code.
* You should modify this routine by adding code which you wish to run fast.
* It will be executed every program loop, and not wait for fresh data
* from the Operator Interface.
* CALLED FROM:   main.c
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void Process_Data_From_Local_IO(void)
{
  /* Add code here that you want to be executed every program loop. */

}

/*******************************************************************************
* FUNCTION NAME: endStage
* PURPOSE:       reset variables and move stage counter up one
* CALLED FROM:   This file
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
/*
void endStage()
{
	int i;

	//reset current position
	for(i; i<3; i++)
	{
		currentPosition[i] = 0;
	}

	//Stop motors for a brief second
	FRONT_LEFT_WHEEL_MOTOR = 127;
	FRONT_RIGHT_WHEEL_MOTOR = 127;
	BACK_LEFT_WHEEL_MOTOR = 127;
	BACK_RIGHT_WHEEL_MOTOR = 127;

	//Reset gear tooth counters
	flEncoder, frEncoder, blEncoder, brEncoder = 0;

	//Move autonomous stage up one
	autonomous_Stage ++;
} */

/*******************************************************************************
* FUNCTION NAME: drive
* PURPOSE:       drive forward of backwards
* CALLED FROM:   This file
* ARGUMENTS:     forwards or backwards, how far
* RETURNS:       void
*******************************************************************************/
void drive(char forwardOrBackward, int howFar){

	virtControlLeftY = 255;
	virtControlLeftX = 127;
	virtControlRightX = 127;
	driveRobot(YES);

}

/*******************************************************************************
* FUNCTION NAME: slide
* PURPOSE:       slide left or right
* CALLED FROM:   This file
* ARGUMENTS:     left or right, how far
* RETURNS:       void
*******************************************************************************/
/*
void slide(char leftOrRight, int howFar){
	//If you want to go left, make goal negative
	if(leftOrRight != RIGHT){
		howFar = -howFar;
	}

	getPID(howFar, currentPosition[0], &virtControlLeftX);//get PID
	driveRobot(YES); //Run robot in autonomous

	//If we get within 5 counts of our goal, end the stage
	if (difference(howFar, currentPosition[0]) <= 5){
		endStage();
	}
} */

/*******************************************************************************
* FUNCTION NAME: turn
* PURPOSE:       turn left or right
* CALLED FROM:   This file
* ARGUMENTS:     left or right, how many degrees in radians
* RETURNS:       void
*******************************************************************************/
/*
void turn(char leftOrRight, float degrees){
	//convert degree to radians and mili it
	int radians = ((degrees * PI)*1000) / 180;
	radians = (int)radians;

	if(leftOrRight != RIGHT){
		radians = -radians;
	}

	getPID(radians, currentPosition[2], &virtControlRightX);
} */

/*******************************************************************************
* FUNCTION NAME: lift
* PURPOSE:       lift arm up or down
* CALLED FROM:   This file
* ARGUMENTS:     up or down, distance
* RETURNS:       void
*******************************************************************************/
/*
void lift(char upOrDown, int destination){
	//liftPosition = Get_Analog_Value(rc_ana_in01);
	//LEFT_LIFT_CLAW_MOTOR = (destination - liftPosition);
	if(upOrDown != UP){
		destination = -destination;
	}

	if (difference(destination, currentPosition[0]) <= 5){
		endStage();
	}
}*/

/*******************************************************************************
* FUNCTION NAME: hoard
* PURPOSE:       moves balls into a more workable position
* CALLED FROM:   This file
* ARGUMENTS:     left or right, time
* RETURNS:       void
*******************************************************************************/
/*void hoard(char leftOrRight, int howFar){
	if(leftOrRight != RIGHT){
		howFar = -howFar;
	}

	if (difference(howFar, currentPosition[0]) <= 5){
		endStage();
	}
}*/

/*******************************************************************************
* FUNCTION NAME: push
* PURPOSE:       knocks balls off the wall
* CALLED FROM:   This file
* ARGUMENTS:     left or right, time
* RETURNS:       void
*******************************************************************************/
/*void push(char forwardOrBackward, int howFar){
	if(forwardOrBackward != FORWARD){
		howFar = -howFar;
	}

	if (difference(howFar, currentPosition[1]) <= 5){
		endStage();
	}
}*/

/*******************************************************************************
* FUNCTION NAME: liftFork
* PURPOSE:
* CALLED FROM:   This file
* ARGUMENTS:     up or down, time
* RETURNS:       void
*******************************************************************************/
/*void liftFork(signed int destination, signed int currentPosition){

//TODO: Fix liftFork.

	if (difference(destination, currentPosition[0]) <= 5){
		endStage();
	}
}*/
